
#pragma once

#include "interface/controller.h"
#include "path_follow_controller.h"

#include <thread>

class PurePursuitController : public PathFollowController {
public:
    PurePursuitController() {};
    void update(const std::vector<Odometry> &poses) override { pose_ = poses[0]; };

    /** PathFollowController **/
    float CalcSteeringAngle(int lookahead_idx) override;

    float CalcLinearVel(float steer_angle) override;

    void FollowThread() override;

    int findLookaheadPoint(int start_idx);

private:
    const float lookahead_dist_ = 0.8;
};
